#include "Matrix4x4.h"
#include <algorithm>

namespace RayTracer
{
    Matrix4x4::Matrix4x4()
    {
	for(int j = 0; j < 4; ++j)
	{
	    for(int i = 0; i < 4; ++i)
	    {
		this->values[i][j] = 0;
	    }
	}

	this->values[0][0] = 1.0f;
	this->values[1][1] = 1.0f;
	this->values[2][2] = 1.0f;
    }

    Matrix4x4::Matrix4x4(float row1[], float row2[], float row3[], float row4[])
    {
	std::copy(row1, row1 + 4, this->values[0]);
	std::copy(row2, row2 + 4, this->values[1]);
	std::copy(row3, row3 + 4, this->values[2]);
	std::copy(row4, row4 + 4, this->values[3]);
    }

    float Matrix4x4::Get11() const
    {
	return this->values[0][0];
    }

    float Matrix4x4::Get12() const
    {
	return this->values[0][1];
    }

    float Matrix4x4::Get13() const
    {
	return this->values[0][2];
    }

    float Matrix4x4::Get14() const
    {
	return this->values[0][3];
    }

    float Matrix4x4::Get21() const
    {
	return this->values[1][0];
    }

    float Matrix4x4::Get22() const
    {
	return this->values[1][1];
    }

    float Matrix4x4::Get23() const
    {
	return this->values[1][2];
    }

    float Matrix4x4::Get24() const
    {
	return this->values[1][3];
    }

    float Matrix4x4::Get31() const
    {
	return this->values[2][0];
    }

    float Matrix4x4::Get32() const
    {
	return this->values[2][1];
    }

    float Matrix4x4::Get33() const
    {
	return this->values[2][2];
    }

    float Matrix4x4::Get34() const
    {
	return this->values[2][3];
    }

    float Matrix4x4::Get41() const
    {
	return this->values[3][0];
    }

    float Matrix4x4::Get42() const
    {
	return this->values[3][1];
    }

    float Matrix4x4::Get43() const
    {
	return this->values[3][2];
    }

    float Matrix4x4::Get44() const
    {
	return this->values[3][3];
    }

    Matrix4x4 Matrix4x4::CreateRotationMatrix(float xAngle, float yAngle, float zAngle)
    {
	Matrix4x4 xRot = Matrix4x4::CreateXAxisRotationMatrix(xAngle);
	Matrix4x4 yRot = Matrix4x4::CreateYAxisRotationMatrix(yAngle);
	Matrix4x4 zRot = Matrix4x4::CreateZAxisRotationMatrix(zAngle);

	Matrix4x4 tmp = Matrix4x4::Mul(&yRot, &zRot);
	return Matrix4x4::Mul(&xRot, &tmp);
    }

    Matrix4x4 Matrix4x4::CreateTranslationMatrix(float dx, float dy, float dz)
    {
	return Matrix4x4();
    }

    Matrix4x4 Matrix4x4::CreateXAxisRotationMatrix(float angle)
    {
	return Matrix4x4();
    }

    Matrix4x4 Matrix4x4::CreateYAxisRotationMatrix(float angle)
    {
	return Matrix4x4();
    }

    Matrix4x4 Matrix4x4::CreateZAxisRotationMatrix(float angle)
    {
	return Matrix4x4();
    }
    
    Vector3D Matrix4x4::Mul(const Matrix4x4 *m, const Vector3D *v)
    {
	Vector3D col0 = m->GetColumn(0);
	Vector3D col1 = m->GetColumn(1);
	Vector3D col2 = m->GetColumn(2);
	
	return Vector3D(Vector3D::Dot(v, &col0), Vector3D::Dot(v, &col1), Vector3D::Dot(v, &col2));
    }

    Matrix4x4 Matrix4x4::Mul(const Matrix4x4 *m1, const Matrix4x4 *m2)
    {
	Vector3D row1 = m1->GetRow(0);
	Vector3D row2 = m1->GetRow(1);
	Vector3D row3 = m1->GetRow(2);
	Vector3D row4 = m1->GetRow(3);

	Vector3D col1 = m2->GetColumn(0);
	Vector3D col2 = m2->GetColumn(1);
	Vector3D col3 = m2->GetColumn(2);
	Vector3D col4 = m2->GetColumn(3);
	
	float resRow1[] = 
	    {
		Vector3D::Dot(&row1, &col1),
		Vector3D::Dot(&row1, &col2),
		Vector3D::Dot(&row1, &col3),
		Vector3D::Dot(&row1, &col4)
	    };
	
	float resRow2[] = 
	{
	    Vector3D::Dot(&row2, &col1),
	    Vector3D::Dot(&row2, &col2),
	    Vector3D::Dot(&row2, &col3),
	    Vector3D::Dot(&row2, &col4)
	};
	
	float resRow3[] = 
	{
	    Vector3D::Dot(&row3, &col1),
	    Vector3D::Dot(&row3, &col2),
	    Vector3D::Dot(&row3, &col3),
	    Vector3D::Dot(&row3, &col4)
	};

	float resRow4[] = 
	{
	    Vector3D::Dot(&row4, &col1),
	    Vector3D::Dot(&row4, &col2),
	    Vector3D::Dot(&row4, &col3),
	    Vector3D::Dot(&row4, &col4)
	};
	
	return Matrix4x4(resRow1, resRow2, resRow3, resRow4);
    }

    Vector3D Matrix4x4::GetRow(int index) const
    {
	return Vector3D(this->values[index][0], this->values[index][1], this->values[index][2]);
    }

    Vector3D Matrix4x4::GetColumn(int index) const
    {
	return Vector3D(this->values[0][index], this->values[1][index], this->values[2][index]);
    }

    float Matrix4x4::Determinant() const
    {
	return 0.0f;
    }
}
